﻿using System;
using System.Collections.Generic;
using System.Text;
using GeometryCSharp;

namespace GeometryCSharp
{
    public class BezierCurve
    {
        Point2D a;
        Point2D b;
        Point2D c;
        Point2D d;
        bool valid;
        public BezierCurve(Point2D a, Point2D b, Point2D c, Point2D d)
        {
            this.a = a;
            this.b = b;
            this.c = c;
            this.d = d;
            valid = true;
        }
        public BezierCurve(bool valid)
        {
            this.valid = valid;
        }
        public bool isValid() {
            return this.valid;
        }
        public Point2D evaluate(double t)
        {
            double x = a.x * (1 - t) * (1 - t) * (1 - t) + 3 * b.x * t * (1 - t) * (1 - t) + 3 * c.x * t * t * (1 - t) + d.x * t * t * t;
            double y = a.y * (1 - t) * (1 - t) * (1 - t) + 3 * b.y * t * (1 - t) * (1 - t) + 3 * c.y * t * t * (1 - t) + d.y * t * t * t;
            return new Point2D(x, y);
        }

        public double calculateLength(int steps)
        {
            double result=0;
            Point2D tmp1, tmp2;
            tmp1 = new Point2D(a.x,a.y);
            for (int i = 0; i < steps; i++)
            {
                tmp2 = evaluate((double)i / (double) steps);
                result += tmp1.GetDistance(tmp2);
                tmp1 = tmp2;
            }
            return result;
        }
   /*     static void Main()
        {
            BezierCurve bc = new BezierCurve(new Point2D(1.2,0.9), new Point2D(1.201, 0.899), new Point2D(1.65, 0.25), new Point2D(1.85, 0.45));
            Console.WriteLine(bc.calculateLength(10));
            Console.WriteLine(bc.calculateLength(100));
            Console.WriteLine(bc.calculateLength(1000));
            Console.WriteLine(bc.calculateLength(10000));
            Console.WriteLine(bc.ToString());
            
            Console.ReadKey();
        }
    */
        public override string ToString()
        {
    //        return a + " " + b + " ---> " + c + " " + d;
            StringBuilder sb = new StringBuilder();
            for (int i = 0; i < 101; i++)
            {
                sb.Append(evaluate((double)i / 100.0));
                sb.Append("\n");
            }
            return sb.ToString();
        }

        public Boolean isToCloseForBezier()
        {
            double x = a.GetDistance(d);
            for (int i = 0; i < 10; i++)
            {
                //liczymy distance - powinno dzialac
                double minX, minY, maxX, maxY;
                minX = a.x;
                maxX  = a.x;
                minY = a.y;
                maxY = a.y;
                if (d.x > a.x)
                {
                    maxX = d.x;
                    minX = a.x;
                }
                else
                {
                    maxX = a.x;
                    minX = d.x;
                }
                if (d.y > a.y)
                {
                    maxY = d.y;
                    minY = a.y;
                }
                else
                {
                    maxY = a.y;
                    minY = d.y;
                }
                Point2D point = evaluate((double)i/10);
                if (point.x > maxX || point.x < minX || point.y > maxY || point.y < minY)
                {
                    return true;
                }
            }
            return false;
        }
        public Boolean isInPitch(double height, double widht,double robotDiameter)
        {
            for (int i = 0; i < 10; i++)
            {
                Point2D point = evaluate((double)i/10);
                if (point.x < 0+robotDiameter || point.x >=  widht-robotDiameter)
                {
                    return false;
                }
                if (point.y < 0+robotDiameter || point.y >= height-robotDiameter)
                {
                    return false;
                }

            }
            return true;
        }

        private static void calculateParameters(ref double a, ref double b, Point2D u1,Point2D u2)
        {
            double x1 = u1.x;
            double y1 = u1.y;
            double x2 = u2.x;
            double y2 = u2.y;
            a = (x1 - x2) / (y2 - y1);
            b = y1 - x1 * a;

        }

       public static Point2D calculateCircleMiddlePoint(Point2D u1, Point2D u2, Point2D v1)
        {
           double acc = 0.0001;
           double a1 = 0, a2 = 0, b1 = 0, b2 = 0,x0=0, y0=0;
           byte case1 = 0;
           // pierwszy wektor jest prostopadly do OX
           if (Math.Abs(u1.x-u2.x) < acc)
           {
               y0 = u1.y;
               case1 = 1;
           }
           // pierwszy wektor rownolegly do ox;
           if (Math.Abs(u1.y - u2.y)<acc)
           {
               x0 = u1.x;

               case1 = 2;
           }
           Point2D z3 = new Point2D((u1.x + v1.x) / 2.0, (u1.y + v1.y) / 2.0);
           // drugi wektor jest prostopadly do ox
           if (z3.x == v1.x)
           {
               if (case1==1)
               {
                   //jazda na wprost !!
                   //trzeba ja obsluzyc wyzej
                   return null;
               }
               else
               {
                   y0 = z3.y;
                   case1 = 3;
               }
               
           }
           // pierwszy wektor rownolegly do ox;
           if (z3.y == v1.y)
           {
               if (case1==2)
               {
                   //jazda na wprost !!
                   //trzeba ja obsluzyc wyzej
                   return null;
               }
               else
               {
                   x0 = z3.x;
                   case1 = 4;
               }
           }
           switch (case1)
           {
               case 1:
                   calculateParameters(ref a2, ref b2, z3, v1);
                   x0 = (y0 - b2) / a2;
                   break;
               case 2:
                   calculateParameters(ref a2, ref b2, z3, v1);
                   y0 = a2 * x0 + b2;
                   break;
               case 3:
                   calculateParameters(ref a1, ref b1, u1, u2);
                   x0 = (y0 - b1) / a1;
                   break;
               case 4:
                   calculateParameters(ref a1, ref b1, u1, u2);
                   y0 = a1 * x0 + b1;
                   break;
               default :
                   calculateParameters(ref a1, ref b1, u1, u2);
                   calculateParameters(ref a2, ref b2, z3, v1);
                   x0 = (b2 - b1) / (a1 - a2);
                   y0 = a1 * x0 + b1;
                   break;


           }
           return new Point2D(x0, y0);
          
        }

    }
}
